import RPi.GPIO as GPIO
from time import sleep

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

# 水平旋转角度转换到PWM占空比
def angleToDutyCycle(angle):
	return 2.5 + (angle / 270.0 )* 10

# 垂直旋转角度转换到PWM占空比
# 190度 回正
# 90度 下落
def angleToDutyCycleVertical(angle):
	return (angle / 270.0 )* 10

def getP(servo_pin):
    GPIO.setup(servo_pin, GPIO.OUT, initial=False)
    p=GPIO.PWM(servo_pin, 50) # 初始频率为50HZ
    sleep(0.5)
    # p.ChangeDutyCycle(0)  # 清空当前占空比，使舵机停止抖动
    return p

p17=getP(17)
p18=getP(18)

# 获取角度
def get_angle(cls_name):
    # 分类角度
    cls_angle={
        'other': 5,
        'recyclable': 90,
        'kitchen': 180,
        'hazardous': 265
    }

    if cls_name=='recyclable':
        return cls_angle['recyclable']
    elif cls_name=='hazardous':
        return cls_angle['hazardous']
    elif cls_name=='kitchen':
        return cls_angle['kitchen']
    else:
        return cls_angle['other']

# servo_pin 舵机信号线接树莓派GPIO17
# 水平转动
def horizontal_change(angle):
    p = p17
    p.start(angleToDutyCycle(angle))
    return p

# 垂直回正
def vertical_change_0():
    p=p18
    p.start(angleToDutyCycle(120))  # 舵机初始化角度为120 回正，回到水平位置

# 垂直倒转
def vertical_change():
    p=p18
    p.start(angleToDutyCycle(120))  # 舵机初始化角度为120 回正，回到水平位置
    sleep(0.8)
    p.ChangeDutyCycle(angleToDutyCycle(0))  # 下降90度
    sleep(1)
    p.start(angleToDutyCycle(120))  # 舵机初始化角度为120  回正
    return p



# 完成分类投放动作
def throw_waste_to_bucket(cls_name):
    # 获取类别角度
    angle = get_angle(cls_name)
    # 水平转动
    p = horizontal_change(angle)
    sleep(1.5)

    # 垂直方向回正
    p = vertical_change()


if __name__ == '__main__':
    for angle in ['other','recyclable','kitchen','hazardous']:
        throw_waste_to_bucket(angle)
    pass